from joint import Joint
from vec3 import Vec3
from quat import Quat
from matrix import Matrix
from spring import Spring
from pointMass import PointMass

import math

class RigidJoint(Joint):
    def __init__(self, fromMass, toMass, slices = 4, stiffness = 1):
        Joint.__init__(self, fromMass, toMass)

        fromRadius = fromMass.getBoundingRadius()
        fromCenter = fromMass.getCenter()

        toRadius = toMass.getBoundingRadius()
        toCenter = toMass.getCenter()

        diff = toCenter - fromCenter
        diffPerp = diff.makePerp()
        
        diffRot = Vec3.UNIT_X.rotationTo(diff.norm())
        
        edgeRot = Quat.fromAxisAngle(diffPerp,math.pi/5)
        
        fromPoints = []
        
        for i in range(slices):
            th = float(i) / float(slices) * math.pi * 2
            spinRot = Quat.fromAxisAngle(diff.norm(),th)
            spinPos = fromCenter + spinRot * edgeRot * diffRot * Vec3.UNIT_X * fromRadius
            fromPoints.append(spinPos)

        diff = fromCenter - toCenter
        diffPerp = diff.makePerp()
        
        diffRot = Vec3.UNIT_X.rotationTo(diff.norm())
        
        edgeRot = Quat.fromAxisAngle(diffPerp,math.pi/5)
        
        toPoints = []
        
        for i in range(slices):
            th = float(i) / float(slices) * math.pi * 2
            spinRot = Quat.fromAxisAngle(diff.norm(),-th)
            spinPos = toCenter + spinRot * edgeRot * diffRot * Vec3.UNIT_X * toRadius
            toPoints.append(spinPos)
            
        fromPms = [fromMass.closestPointMassTo(p) for p in fromPoints]
        toPms = [toMass.closestPointMassTo(p) for p in toPoints]
        
        #for i in range(slices):
            #pm = PointMass(fromPoints[i])
            #self.pointMasses.append(pm)
            #self.springs.append(Spring(fromMass.closestPointMassTo(pm.pos),pm))
            
        for i in range(slices):
            next = (i + 1) % slices
            
            #self.springs.append(Spring(fromPms[i],fromPms[next],stiffness = stiffness))
            #self.springs.append(Spring(toPms[i],toPms[next],stiffness = stiffness))
            self.springs.append(Spring(fromPms[i],toPms[i],stiffness = stiffness))
            self.springs.append(Spring(fromPms[i],toPms[next],stiffness = stiffness))
            self.springs.append(Spring(toPms[i],fromPms[next],stiffness = stiffness))
